import math
import time
import traceback
from Serial_Control import SerialControl
from config import port
init_radObjs =   [[1,0],[2,0.5], [3,0.3],[4,0],  [5,0.6],[6,-0.6],[7,0],[8,0.7],[9,-0.4],[10,0],[11,0.6],[12,-0.6],[13,0],[14,0.6],[15,-0.5],[16,0.19],[17,0.8],[18,-2.4]]
up_degs=         [[1,55],[2,70], [3,135],[4,-10],[5,0],  [6,110],[7,40],[8,70],[9,135],[10,80],[11,90],[12,135],[13,45],[14,70],[15,135],[16,100],[17,80],[18,70]]

stand_degs=      [[1,55],[2,70], [3,-30],[4,-10],[5,0],  [6,-90],[7,40],[8,70],[9,-70],[10,80],[11,80],[12,-80],[13,45],[14,70],[15,-90],[16,100],[17,80],[18,-80]]
# stand_degs_rads= [[1,55],[2,70], [3,-30],[4,-10],[5,0],  [6,-90],[7,40],[8,70],[9,-70],[10,80],[11,80],[12,-80],[13,45],[14,70],[15,-90],[16,100],[17,80],[18,-80]]
right_step1_degs= [[1,70],[2,70], [3,-30],[4,-30],[5,20], [6,-105],[7,40],[8,80],[9,-70],[10,80],[11,100],[12,-80],[13,0],[14,60],[15,-75],[16,70],[17,90],[18,-70]]
right_step0_degs=[[stand_degs[i][0],stand_degs[i][1]+right_step1_degs[i][1]] for i in range(len(stand_degs))]
right_step2_degs=[[1,90],[2,70],[3,-30],[4,-30],[5,20], [6,-90],[7,60],[8,50],[9,-60],[10,100],[11,70],[12,-80],[13,0],[14,80],[15,-90],[16,70],[17,70],[18,-80]]

class legMotion:
    def __init__(self,ser:SerialControl):
        self.ser = ser
        # self.ser.servoRads(init_radObjs)#type: ignore
    def interact(self):
        while True:
            try:
                iAndDegree = input("请输入关节编号和角度：")
                if iAndDegree=="=":
                    return
                i,degree = iAndDegree.split(" ")
                if degree=="SCK":
                    self.ser.servoRads([(int(i),degree)])
                else:
                    rad = float(degree) * math.pi / 180
                    self.ser.servoRads([(int(i),rad)])
                
            except Exception as e:
                print(e,traceback.format_exc())
                continue
    def legsTogether(self):
        leftLegs  = [1, 7, 13]
        rightLegs = [4, 10, 16]
        self.ser.servoRads(init_radObjs,surpress_print=True)
        radObjs = init_radObjs.copy()
        # Mapping: key -> [offset_in_leg_section, delta_angle]
        left_map = {
            "1":[0,  0.1], "q":[0, -0.1],
            "2":[1,  0.1], "w":[1, -0.1],
            "3":[2,  0.1], "e":[2, -0.1],
        }
        right_map = {
            "4":[0,  0.1], "r":[0, -0.1],
            "5":[1,  0.1], "t":[1, -0.1],
            "6":[2,  0.1], "y":[2, -0.1],
        }

        while True:
            keys = input("enter the keys:")
            if keys=="reboot":
                self.ser.servoRads(init_radObjs,surpress_print=True)
                radObjs = init_radObjs.copy()
                continue
            for key in keys:
                # If the key is in the left_map, adjust each servo in leftLegs
                if key == "=":
                    return
                if key in left_map:
                    offset, angle_delta = left_map[key]
                    for l in leftLegs:
                        # Subtract 1 because servo #1 is stored at radObjs[0], etc.
                        radObjs[(l - 1) + offset][1] += angle_delta
                # If the key is in the right_map, adjust each servo in rightLegs
                if key in right_map:
                    offset, angle_delta = right_map[key]
                    for r in rightLegs:
                        radObjs[(r - 1) + offset][1] += angle_delta

            # Send updated angles
            self.ser.servoRads(radObjs,surpress_print=True)
    def keyBoard(self):
        # Use lists (instead of tuples) so that angles can be updated in-place
        # If your servoRads() expects tuples, convert just before sending
        self.ser.servoRads(init_radObjs)
        leftLegs  = [1, 2, 3, 7, 8, 9, 13, 14, 15]
        rightLegs = [4, 5, 6, 10, 11, 12, 16, 17, 18]
        radObjs=init_radObjs.copy()
        while True:
            keys = input("enter the keys:")
            if keys=="=":
                return
            if keys=="-":
                self.ser.servoRads(init_radObjs)
                radObjs=init_radObjs.copy()
                continue
            for key in keys:
                delta_map = {
                    "1": [1, 0.1], "q": [1, -0.1],
                    "2": [2, 0.1], "w": [2, -0.1],
                    "3": [3, 0.1], "e": [3, -0.1],
                    "4": [4, 0.1], "r": [4, -0.1],
                    "5": [5, 0.1], "t": [5, -0.1],
                    "6": [6, 0.1], "y": [6, -0.1],
                    "7": [7, 0.1], "u": [7, -0.1],
                    "8": [8, 0.1], "i": [8, -0.1],
                    "9": [9, 0.1], "o": [9, -0.1],
                    "a": [10, 0.1], "z": [10, -0.1],
                    "s": [11, 0.1], "x": [11, -0.1],
                    "d": [12, 0.1], "c": [12, -0.1],
                    "f": [13, 0.1], "v": [13, -0.1],
                    "g": [14, 0.1], "b": [14, -0.1],
                    "h": [15, 0.1], "n": [15, -0.1],
                    "j": [16, 0.1], "m": [16, -0.1],
                    "k": [17, 0.1], ",": [17, -0.1],
                    "l": [18, 0.1], ".": [18, -0.1]
                }
                # group_list =["1f7","2g8","3h9","qvu","wbi","eno","a4j","s5k","d6l","zrm","xt,","ct."] 
                # for group in group_list:
                #     if key in group:
                #         group_keys = group
                #         group_keys = group_keys.replace(key,"")
                #         keys+=group_keys
                #         print(f"key {key} in group {group} keys change to {keys}")

                if key not in delta_map:
                    print("Unknown key:", key)
                    continue

                delta = delta_map[key]
                radObjs[delta[0] - 1][1] += delta[1]
                print(radObjs[delta[0] - 1])
            self.ser.servoRads(radObjs,surpress_print=True)
    def degsToRad(self,degs):
        return [[deg[0],float(deg[1])*math.pi/180] for deg in degs]
    def stand(self):
        rads=self.degsToRad(stand_degs)
        self.ser.servoRads(rads,use_seq=False)
    def up(self):
        rads=self.degsToRad(up_degs)
        self.ser.servoRads(rads,use_seq=False)
    def rightStep0(self):
        rads=self.degsToRad(right_step0_degs)
        self.ser.servoRads(rads,use_seq=False)
    def rightStep1(self):
        rads=self.degsToRad(right_step1_degs)
        self.ser.servoRads(rads,use_seq=False)
    def rightStep2(self):
        rads=self.degsToRad(right_step2_degs)
        self.ser.servoRads(rads,use_seq=False)
if __name__ == "__main__":
    legmotion = legMotion(SerialControl(port,115200))
    while True:
        mode=input("enter the mode:")
        # stand=[0.9094910732142452, -0.5348561492736622, -1.1733848561157876, -0.8293804605477054, 1.5810065029190634, -1.4231414720761764, 0.6668030407244335, 1.32653749797829, -1.5527321690367553, 1.646979948644449, 1.503252084742716, -1.5079644737231006]
        # comtest.ser.servoRads([[i,stand[i]] for i in range(1,19)])
        if mode=="s":
            legmotion.stand()
        elif mode=="u":
            legmotion.up()
        elif mode=="r0":
            legmotion.rightStep0()
        elif mode=="r1":
            legmotion.rightStep1()
        elif mode=="r2":
            legmotion.rightStep2()
        elif mode=="i":
            legmotion.interact()
        elif mode=="k":
            legmotion.keyBoard()
        elif mode=="t":
            legmotion.legsTogether()
        elif mode=="rl":
            print(legmotion.ser.get_rads_data("legs"))
        elif mode=="ra":
            print(legmotion.ser.get_rads_data("arm"))


